#include "FrenetOptimalTrajectory.h"

FrenetOptimalTrajectory::FrenetOptimalTrajectory() {}

FrenetOptimalTrajectory::~FrenetOptimalTrajectory() {}

void FrenetOptimalTrajectory::calc_frenet_paths(FrenetInitialConditions* fot_ic, FrenetHyperparameters* fot_hp,
                    std::priority_queue<FrenetPath*, std::vector<FrenetPath*>, CostComparator_for_Frenet>& frenet_path, 
                    std::vector<std::pair<double, double>>& global_points, std::vector<double>& accumulated_s, int& vehicle_index, 
                    const std::unique_ptr<CubicSpline2D>& Cub_spline_2D, const std::vector<std::pair<double, double>>& s_d_point)
{
    double t, ti, tv;   // t从0开始，间隔DT,加到ti，tv：纵向速度规划
    double lateral_deviation, lateral_velocity, lateral_acceleration, lateral_jerk;
    double longitudinal_acceleration, longitudinal_jerk;
    double di;    //横向宽度采样
    //%调用类，FrenetPath里面定义了一系列参数
    //这里分两条写了。tfp要复制fp有的参数值，最后使用tfp就可以
    //生成每个偏移目标的路径，即d-t坐标系下的路径轨迹和s-t下的速度轨迹
    if(abs(fot_hp->Min_road_width) >= 1.0){
        di = fot_hp->Min_road_width;
    }else{
        di = fot_hp->middle_road_;
    }
    while (di <= fot_hp->Max_road_width + fot_hp->D_road_w)
    {
        ti = fot_hp->Mint;
        while (ti <= fot_hp->Maxt + fot_hp->Dt)
        {
            //初始化用于累加的参数
            lateral_deviation = 0;
            lateral_velocity = 0;
            lateral_acceleration = 0;
            lateral_jerk = 0;
            FrenetPath *fp = new FrenetPath();
            // d0, dd0, ddd0为初始配置， di, ddT, dddT为目标配置
            //计算出关于目标配置di，Ti的横向多项式，5次多项式
            QuinticPolynomial lat_qp = QuinticPolynomial(fot_ic->d0, fot_ic->dd0, fot_ic->ddd0, di, 0, 0, ti);
            // 构建frenet路径，t从0开始，间隔DT,加到ti
            t = 0.0;
            while (t <= ti + fot_hp->Dt) //其实就是0-ti
            {
                //存储d/t坐标系的值
                fp->t.push_back(t);                                                                                //时间戳存储，这样写是生成数组
                fp->d.push_back(lat_qp.quint_point(t));                                    //存储横向偏移
                fp->d_d.push_back(lat_qp.quint_first_derivative(t));           //取一阶导数值
                fp->d_dd.push_back(lat_qp.quint_first_derivative(t));        //取二阶导数值
                fp->d_ddd.push_back(lat_qp.quint_third_derivative(t));   //取三阶导数值
                //累加，为了求代价函数
                lateral_deviation += fabs(lat_qp.quint_point(t));                                    //横向偏差累加
                lateral_velocity += fabs(lat_qp.quint_first_derivative(t));                   //横向速度累加
                lateral_acceleration += fabs(lat_qp.quint_first_derivative(t));          //横向加速度累加
                lateral_jerk += fabs(lat_qp.quint_third_derivative(t));                         //横向加加速度累加
                t += fot_hp->Dt;                                                                                                      //加Dt,循环
            }
            tv = fot_ic->target_speed - fot_hp->D_t_s * fot_hp->N_s_sample;
            while (tv <= fot_ic->target_speed + fot_hp->D_t_s * fot_hp->N_s_sample + fot_hp->D_t_s)
            {
                longitudinal_acceleration = 0;
                longitudinal_jerk = 0;
                // copy frenet path
                FrenetPath *tfp = new FrenetPath();
                tfp->t.assign(fp->t.begin(), fp->t.end());
                tfp->d.assign(fp->d.begin(), fp->d.end());
                tfp->d_d.assign(fp->d_d.begin(), fp->d_d.end());
                tfp->d_dd.assign(fp->d_dd.begin(), fp->d_dd.end());
                tfp->d_ddd.assign(fp->d_ddd.begin(), fp->d_ddd.end());
                //四次多项式
                QuarticPolynomial lon_qp = QuarticPolynomial(fot_ic->s0, fot_ic->ds0, 0.0, tv, 0.0, ti);
                // 纵向规划
                for (double tp : tfp->t) //其实就是0-ti,tfp->t为数组，每个值间隔Dt
                {
                    //存储s/t坐标系的值
                    tfp->s.push_back(lon_qp.quart_point(tp));                //取值
                    tfp->s_d.push_back(lon_qp.quart_first_derivative(tp));   //取一阶导数值
                    tfp->s_dd.push_back(lon_qp.quart_second_derivative(tp)); //取二阶导数值
                    tfp->s_ddd.push_back(lon_qp.quart_third_derivative(tp)); //取三阶导数值
                    //累加，为了求代价函数
                    longitudinal_acceleration += fabs(lon_qp.quart_second_derivative(tp));
                    longitudinal_jerk += fabs(lon_qp.quart_third_derivative(tp));
                }
                // 失败或路径无效时删除
                //每次生成一条轨迹,就进行检测,检测不过之间删除并跳到下一个循环
                bool success = to_global_path(*tfp, global_points, accumulated_s, vehicle_index, Cub_spline_2D);
                if (!success || !is_valid_path(*tfp, s_d_point))
                {
                    // 释放内存并继续
                    delete tfp;
                    tv += fot_hp->D_t_s;
                    continue;
                }

                //横向的代价，代价函数待改善
                tfp->c_lateral_deviation = lateral_deviation;
                tfp->c_lateral_velocity = lateral_velocity;
                tfp->c_lateral_acceleration = lateral_acceleration;
                tfp->c_lateral_jerk = lateral_jerk;
                tfp->Jd = fot_hp->kj * tfp->c_lateral_jerk + fot_hp->kd * tfp->c_lateral_deviation +
                                    fot_hp->kv * tfp->c_lateral_velocity + fot_hp->kt * ti;
                /* tfp->wd = fot_hp->wlat_d *  tfp->c_lateral_deviation + fot_hp->wlat_v * tfp->c_lateral_velocity + 
                                            fot_hp->wlat_a * tfp->c_lateral_acceleration + fot_hp->wlat_j * tfp->c_lateral_jerk + fot_hp->wlon_t* ti; */
                //纵向的代价，代价函数待改善
                tfp->c_longitudinal_acceleration = longitudinal_acceleration;
                tfp->c_longitudinal_jerk = longitudinal_jerk;
                tfp->c_end_speed_deviation = pow((fot_ic->target_speed - tfp->s_d.back()), 2); //速度差
                tfp->c_time_taken = ti;
                /* tfp->ws = fot_hp->wlon_a * tfp->c_longitudinal_acceleration + fot_hp->wlon_j * tfp->c_longitudinal_jerk + 
                                                    fot_hp->wlon_v * tfp->c_end_speed_deviation + fot_hp->wlon_t * tfp->c_time_taken;
                tfp->W = fot_hp->wlat_score * tfp->wd + fot_hp->wlon_score * tfp->ws; */
                tfp->Js = fot_hp->kj * tfp->c_longitudinal_jerk + fot_hp->kt * tfp->c_time_taken +
                                                fot_hp->kd * tfp->c_end_speed_deviation + fot_hp->ka * tfp->c_longitudinal_acceleration;
                tfp->J = fot_hp->klat * tfp->Jd + fot_hp->klon * tfp->Js;
                frenet_path.emplace(tfp);

                tv += fot_hp->D_t_s;
            }
            ti += fot_hp->Dt;
            delete fp;
        }
        di += fot_hp->D_road_w;
    }
    

}

// Convert the frenet path to global path in terms of x, y, yaw, velocity
bool FrenetOptimalTrajectory::to_global_path(FrenetPath &fts, std::vector<std::pair<double, double>>& global_points, 
                                                                                                        std::vector<double>& accumulated_s, int& vehicle_index, const std::unique_ptr<CubicSpline2D>& Cub_spline_2D)
{
    double dx, dy;
    if (global_points.size() <= 1)
    {
        return false;
    }

    for(size_t i = 0; i < fts.s.size(); i++)
    {
        double ix_ = Cub_spline_2D->calc_x(fts.s[i]);
        double iy_ = Cub_spline_2D->calc_y(fts.s[i]);
        if (isnan(ix_) || isnan(iy_)) //如果ix_,iy_不存在
            break;
        double iyaw_ = Cub_spline_2D->calc_yaw(fts.s[i]);
        double di = fts.d[i];
        double fx = ix_ + di * cos(iyaw_ + M_PI_2);
        double fy = iy_ + di * sin(iyaw_ + M_PI_2);
        fts.x.push_back(fx);
        fts.y.push_back(fy);
        fts.v.push_back(fts.s_d[i]);
        fts.a.push_back(fts.s_dd[i]);
        fts.kappas.push_back(Cub_spline_2D->calc_curvature(fts.s[i]));
    }

    //没有足够的点来构造有效路径
    if (fts.x.size() <= 1)
    {
        return false;
    }

    // 计算 theta and dL (running length)
    for (size_t i = 0; i < fts.x.size() - 1; i++)
    {
        dx = fts.x[i + 1] - fts.x[i];
        dy = fts.y[i + 1] - fts.y[i];
        fts.theta.push_back(atan2(dy, dx)); //航向角
        fts.dL.push_back(hypot(dx, dy));    //垂直里程
    }

    fts.theta.push_back(fts.theta.back()); //.back()返回vetor最末尾的值
    fts.dL.push_back(fts.dL.back());

    return true;
}

// 检查轨迹是否可行（要满足最大条件）,以及碰撞检测
bool FrenetOptimalTrajectory::is_valid_path(FrenetPath &fts, const std::vector<std::pair<double, double>>& s_d_point)
{
    // max speed check
    if (any_of(fts.s_d.begin(), fts.s_d.end(), [this](int i)
                { return fabs(i) > 3; }))
    {
        return false;
    }
    // max accel check
    else if (any_of(fts.s_dd.begin(), fts.s_dd.end(), [this](int i)
                    { return fabs(i) > 2; }))
    {
        return false;
    }
    // max curvature check
    else if (any_of(fts.kappas.begin(), fts.kappas.end(), [this](int i)
                    { return fabs(i) > 2; }))
    {
        return false;
    }

    for(size_t i = 0; i < fts.s.size(); i++){
        double path_s = fts.s[i];
        double path_d = fts.d[i];
        for(const auto& sd_point : s_d_point){
            double delta_s = std::fabs(path_s - sd_point.first);
            double delta_d = std::fabs(path_d - sd_point.second);

            // 检查 s 和 d 方向是否都在安全距离内
            /* if (delta_s < path_par.vehicle_length / 2 && delta_d < path_par.vehicle_width / 2)
            {
                return false; // 碰撞风险
            } */
        }
    }

    return true;
}